/*
 * @Author: LVGRAPE
 * @Date: 2023-09-06 10:22:03
 * @LastEditTime: 2023-09-07 18:15:06
 * @LastEditors: LVGRAPE
 * @Description:
 * @FilePath: \ZINO_FC_V4\ZINO\hardware\pwr\voltage.c
 * 可以输入预定的版权声明、个性签名、空行等
 */
#include "drv_common.h"
#if defined(BSP_USING_ADC1) && defined(RT_USING_ADC)


#include <rtdevice.h>
#include "drv_gpio.h"
#include "zino.h"
#include "pin.h"
#include "filter.h"
#include <stdbool.h>
#define ADC_DEV_NAME "adc1"
#define DBG_TAG "vol"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

#define VBM_PIN GET_PIN(B,15)
#define V25_MV 1280 // 在25 ℃时的电压1.28V 
#define AVG_SLOPE -4.2f //平均斜率 -4.20Mv/℃
static struct zino_voltage
{
    union voltage
    {
        struct {
            uint16_t vbat;
            uint16_t v5;
            uint16_t v3_3;
            uint16_t v1_8;
            uint16_t m1;
            uint16_t m2;
            uint16_t m3;
            uint16_t m4;
            uint16_t rgb;
            uint16_t temp;
            uint16_t vref;
        };
        uint16_t raw[11];
    }adc;

    float vbat_mv;
    float v5_mv;
    float v3_3_mv;
    float v1_8_mv;
    float m1_ma;
    float m2_ma;
    float m3_ma;
    float m4_ma;
    float rgb_mv;
    float temp_mc;
    float vref_mv;

}__PACKED zinoVol;
static Kalman_t vbatFilter;
static Kalman_t v5Filter;
static Kalman_t v3Filter;
static Kalman_t v1Filter;
static Kalman_t m1Filter;
static Kalman_t m2Filter;
static Kalman_t m3Filter;
static Kalman_t m4Filter;
static bool printon = false;
enum zino_vol_index {
    V_INDEX_V5 = 8,
    V_INDEX_VBAT = 9,
    V_INDEX_V3_3 = 12,
    V_INDEX_V1_8 = 13,
    V_INDEX_M4 = 10,
    V_INDEX_M1 = 11,
    V_INDEX_M2 = 15,
    V_INDEX_M3 = 14,
    V_INDEX_RGB = 6,
    V_INDEX_TEMP = 16,
    V_INDEX_REF = 17,
};

static const rt_uint8_t chs[] = {
    //NOTE 电压采集顺序不能变，需和zinoVol.adc对应
    V_INDEX_VBAT,
    V_INDEX_V5,
    V_INDEX_V3_3,
    V_INDEX_V1_8,
    V_INDEX_M1,
    V_INDEX_M2,
    V_INDEX_M3,
    V_INDEX_M4,
    V_INDEX_RGB,
    // V_INDEX_TEMP,
    // V_INDEX_REF,
};
void voltage_cal(struct zino_voltage* v, enum zino_vol_index ch)
{
    switch (ch)
    {
    case V_INDEX_RGB:v->rgb_mv = v->adc.rgb;
        break;
    case V_INDEX_V5:v->v5_mv = KalmanFilter(&v5Filter, v->adc.v5 * 3300 / 4096 * 2);
        break;
    case V_INDEX_VBAT:v->vbat_mv = KalmanFilter(&vbatFilter, v->adc.vbat * 3300 / 4096 * 2);
        break;
    case V_INDEX_V3_3:v->v3_3_mv = KalmanFilter(&v3Filter, v->adc.v3_3 * 3300 / 4096 * 2);
        break;
    case V_INDEX_V1_8:v->v1_8_mv = KalmanFilter(&v1Filter, v->adc.v1_8 * 3300 / 4096 * 2);
        break;
    case V_INDEX_M1:v->m1_ma = KalmanFilter(&m1Filter, v->adc.m1 * 3300 / 4096 * 33 / (10 + 33) / 0.01f);
        break;
    case V_INDEX_M2:v->m2_ma = KalmanFilter(&m2Filter, v->adc.m2 * 3300 / 4096 * 33 / (10 + 33) / 0.01f);
        break;
    case V_INDEX_M3:v->m3_ma = KalmanFilter(&m3Filter, v->adc.m3 * 3300 / 4096 * 33 / (10 + 33) / 0.01f);
        break;
    case V_INDEX_M4:v->m4_ma = KalmanFilter(&m4Filter, v->adc.m4 * 3300 / 4096 * 33 / (10 + 33) / 0.01f);
        break;
    case V_INDEX_TEMP:v->temp_mc = ((V25_MV - v->adc.temp * 3300 / 4096) / AVG_SLOPE) + 2500;
        break;
    case V_INDEX_REF:v->vref_mv = v->adc.vref * 3300 / 4096;
        break;
    default:
        break;
    }
}

void voltage_mon_task(void* args)
{
    static rt_adc_device_t adc_device = RT_NULL;
    adc_device = (rt_adc_device_t)rt_device_find(ADC_DEV_NAME);
    const rt_uint8_t chs_cnt = sizeof(chs) / sizeof(chs[0]);
    static rt_uint8_t chs_cur = 0;
    if (adc_device == RT_NULL)
    {
        LOG_E("adc1 not found!");
        return;
    }

    rt_adc_enable(adc_device, chs_cur);
    rt_pin_mode(VBM_PIN, PIN_MODE_OUTPUT_OD);
    rt_pin_write(VBM_PIN, 0);
    KalmanFilterInitParam(&vbatFilter, 0.0001, 0.1);
    KalmanFilterInitParam(&v5Filter, 0.0001, 0.1);
    KalmanFilterInitParam(&v3Filter, 0.0001, 0.1);
    KalmanFilterInitParam(&v1Filter, 0.0001, 0.1);
    KalmanFilterInitParam(&m1Filter, 0.0001, 0.1);
    KalmanFilterInitParam(&m2Filter, 0.0001, 0.1);
    KalmanFilterInitParam(&m3Filter, 0.0001, 0.1);
    KalmanFilterInitParam(&m4Filter, 0.0001, 0.1);
    while (1)
    {
        zinoVol.adc.raw[chs_cur] = rt_adc_read(adc_device, chs[chs_cur]);
        // rt_adc_disable(adc_device, chs[chs_cur]);
        voltage_cal(&zinoVol, chs[chs_cur]);


        // if (chs_cur == 0) rt_kprintf("ADC: ");
        // rt_kprintf("CH:%d>%04d \t", chs[chs_cur], zinoVol.adc.raw[chs_cur],);

        chs_cur++;
        if (chs_cur == chs_cnt && printon)
        {
            rt_kprintf("Vbat>%d.%03dV \t", (int)zinoVol.vbat_mv / 1000, (int)zinoVol.vbat_mv % 1000);
            rt_kprintf("v5.0>%d.%03dV \t", (int)zinoVol.v5_mv / 1000, (int)zinoVol.v5_mv % 1000);
            rt_kprintf("v3.3>%d.%03dV \t", (int)zinoVol.v3_3_mv / 1000, (int)zinoVol.v3_3_mv % 1000);
            rt_kprintf("v1.8>%d.%03dV \t", (int)zinoVol.v1_8_mv / 1000, (int)zinoVol.v1_8_mv % 1000);
            rt_kprintf("m1>%d.%03dmA \t", (int)zinoVol.m1_ma / 1000, (int)zinoVol.m1_ma % 1000);
            rt_kprintf("m2>%d.%03dmA \t", (int)zinoVol.m2_ma / 1000, (int)zinoVol.m2_ma % 1000);
            rt_kprintf("m3>%d.%03dmA \t", (int)zinoVol.m3_ma / 1000, (int)zinoVol.m3_ma % 1000);
            rt_kprintf("m4>%d.%03dmA \t", (int)zinoVol.m4_ma / 1000, (int)zinoVol.m4_ma % 1000);
            rt_kprintf("rgb:%d.%04dmV\t", (int)zinoVol.rgb_mv / 1000, (int)zinoVol.rgb_mv % 1000);
            rt_kprintf("\n");
        }

        if (chs_cur >= chs_cnt)
        {
            chs_cur = 0;
        }
        rt_adc_enable(adc_device, chs[chs_cur]);
        rt_thread_mdelay(1);
    }
    // return;
}
int voltage_task_launch()
{
    rt_thread_t vol_threadId = rt_thread_create("vol", voltage_mon_task, RT_NULL, 1024, 25, 10);
    if (vol_threadId)
    {
        rt_thread_startup(vol_threadId);
        LOG_I("voltage_task_launch");
        return RT_EOK;
    }
    else
    {
        LOG_E("voltage_task_launch failed!");
        return RT_ERROR;
    }
    return RT_EOK;
}
float get_vbat_voltage_mv()
{
    return zinoVol.vbat_mv;
}
float get_v5_voltage_mv()
{
    return zinoVol.v5_mv;
}
float get_v3_3_voltage_mv()
{
    return zinoVol.v3_3_mv;
}
float get_m1_current_ma()
{
    return zinoVol.m1_ma;
}
float get_m2_current_ma()
{
    return zinoVol.m2_ma;
}
float get_m3_current_ma()
{
    return zinoVol.m3_ma;
}
float get_m4_current_ma()
{
    return zinoVol.m4_ma;
}
  
long vol(int argc, char** argv)
{
    if (argc == 1)
    {
        rt_kprintf("Vbat>%d.%03dV \t", (int)zinoVol.vbat_mv / 1000, (int)zinoVol.vbat_mv % 1000);
        rt_kprintf("v5.0>%d.%03dV \t", (int)zinoVol.v5_mv / 1000, (int)zinoVol.v5_mv % 1000);
        rt_kprintf("v3.3>%d.%03dV \t", (int)zinoVol.v3_3_mv / 1000, (int)zinoVol.v3_3_mv % 1000);
        rt_kprintf("v1.8>%d.%03dV \t", (int)zinoVol.v1_8_mv / 1000, (int)zinoVol.v1_8_mv % 1000);
        rt_kprintf("m1>%d.%03dmA \t", (int)zinoVol.m1_ma / 1000, (int)zinoVol.m1_ma % 1000);
        rt_kprintf("m2>%d.%03dmA \t", (int)zinoVol.m2_ma / 1000, (int)zinoVol.m2_ma % 1000);
        rt_kprintf("m3>%d.%03dmA \t", (int)zinoVol.m3_ma / 1000, (int)zinoVol.m3_ma % 1000);
        rt_kprintf("m4>%d.%03dmA \t", (int)zinoVol.m4_ma / 1000, (int)zinoVol.m4_ma % 1000);
        rt_kprintf("rgb:%d.%04dmV\t", (int)zinoVol.rgb_mv / 1000, (int)zinoVol.rgb_mv % 1000);
        rt_kprintf("\n");
    }
    else if (argc == 2)
    {
        if (rt_strcmp(argv[1], "raw") == 0)
        {
            rt_kprintf("vbat:%d \t", zinoVol.adc.vbat);
            rt_kprintf("v5.0:%d \t", zinoVol.adc.v5);
            rt_kprintf("v3.3:%d \t", zinoVol.adc.v3_3);
            rt_kprintf("v1.8:%d \t", zinoVol.adc.v1_8);
            rt_kprintf("m1:%d \t", zinoVol.adc.m1);
            rt_kprintf("m2:%d \t", zinoVol.adc.m2);
            rt_kprintf("m3:%d \t", zinoVol.adc.m3);
            rt_kprintf("m4:%d \t", zinoVol.adc.m4);
            rt_kprintf("\n");
        }
        else if (rt_strcmp(argv[1], "show") == 0)
        {
            printon = TRUE;
        }
        else if (rt_strcmp(argv[1], "close") == 0)
        {
            printon = FALSE;
        }
    }
    return RT_EOK;
}
ZINO_APP_EXPORT(voltage_task_launch);
MSH_CMD_EXPORT(vol, show system voltage);
#else
#error "please define DSP_USING_ADC1 and RT_USING_ADC"
#endif
